#include "can_transreceiver.h"
#include "chuansu/mr418.h"
#include "chuansu/sr439.h"
#include "nezha/ep30.h"
#include "sensor_utils.hpp"
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

using namespace sensor::radar;

CanTransreceiver::CanTransreceiver(std::string dev, muduo::net::EventLoop* loop)
    : can_dev_(std::move(dev))
    , loop_(loop)
    , logger_(rclcpp::get_logger("CanTransreceiver"))
{
    signal(SIGPIPE, SIG_IGN);

    /* 打开套接字 */
    int fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (fd < 0) {
        throw std::runtime_error(sensor::utils::stringFormat("can socket create error [%d]%s", errno, strerror(errno)));
    }

    RCLCPP_INFO(logger_, "can socket %s create done!, fd %d", can_dev_.c_str(), fd);

    /* 指定 can0 设备 */
    struct ifreq ifr;
    bzero(&ifr, sizeof(ifr));

    struct sockaddr_can can_addr;
    bzero(&can_addr, sizeof(can_addr));

    strcpy(ifr.ifr_name, can_dev_.c_str());
    ioctl(fd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    int enable_candf = 1;
    if (setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_candf, sizeof(enable_candf)) < 0) {
        close(fd);
        throw std::runtime_error(sensor::utils::stringFormat("can socket bind error [%d]%s", errno, strerror(errno)));
    }

    /* 将 can0 与套接字进行绑定 */
    if (bind(fd, (struct sockaddr*)&can_addr, sizeof(can_addr)) < 0) {
        close(fd);
        throw std::runtime_error(sensor::utils::stringFormat("can socket bind error [%d]%s", errno, strerror(errno)));
    }

    // filter();

    RCLCPP_INFO(logger_, "can socket %s bind done!", can_dev_.c_str());

    chnl_ = std::make_unique<muduo::net::Channel>(loop_, fd);
    chnl_->setReadCallback(std::bind(&CanTransreceiver::onDataReceived, this));
    // if (can_dev_ == "can1") {
    // }
    RCLCPP_INFO(logger_, "can socket %s channel create done!", can_dev_.c_str());
}

void CanTransreceiver::start()
{
    loop_->runInLoop([&] { chnl_->enableReading(); });
    RCLCPP_INFO(logger_, "can socket %s channel start enableReading!", can_dev_.c_str());
}

void CanTransreceiver::filter(std::map<unsigned uint16_t, unsigned int> canIdMap)
{
    struct can_filter rfilter[canIdMap.size()];
    int index = 0;
    for (auto& it : canIdMap) {
        rfilter[index].can_id = it.first;
        rfilter[index].can_mask = it.second;
        index++;
    }
    if (setsockopt(chnl_->fd(), SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)) < 0) {
        throw std::runtime_error(sensor::utils::stringFormat("can socket bind error [%d]%s", errno, strerror(errno)));
    }
}

void CanTransreceiver::filter()
{
    /********************* 过滤规则设置 *********************/
    // CAN_SFF_MASK 0x000007FFU 定义在 can.h 中 (SFF: standard frame format)
    struct can_filter rfilter[3]; // 定义过滤规则数组，结构体 can_filter 是库函数定义好的
    rfilter[0].can_id = EP30_ESC_FR07_291_FRAME_ID;
    rfilter[0].can_mask = CAN_SFF_MASK; // 标准帧 (SFF: standard frame format)

    rfilter[1].can_id = EP30_VCU_FR05_223_FRAME_ID;
    rfilter[1].can_mask = CAN_SFF_MASK; // 标准帧

    rfilter[2].can_id = EP30_APA_FR03_27_B_FRAME_ID;
    rfilter[2].can_mask = CAN_SFF_MASK; // 标准帧

    // rfilter[3].can_id = MR418_EXTENDED_INFORMATION_FRAME_ID;
    // rfilter[3].can_mask = CAN_SFF_MASK; // 标准帧

    // rfilter[4].can_id = MR418_GENERAL_INFORMATION_FRAME_ID;
    // rfilter[4].can_mask = CAN_SFF_MASK; // 标准帧

    // rfilter[5].can_id = SR439_OBJ_EXTENDED_INFO_FL_FRAME_ID;
    // rfilter[5].can_mask = CAN_EFF_MASK; // 扩展帧

    // rfilter[6].can_id = SR439_OBJ_EXTENDED_INFO_FR_FRAME_ID;
    // rfilter[6].can_mask = CAN_EFF_MASK; // 扩展帧

    // rfilter[7].can_id = SR439_OBJ_GENERAL_INFO_FL_FRAME_ID;
    // rfilter[7].can_mask = CAN_EFF_MASK; // 扩展帧

    // rfilter[8].can_id = SR439_OBJ_GENERAL_INFO_FR_FRAME_ID;
    // rfilter[8].can_mask = CAN_EFF_MASK; // 扩展帧

    // rfilter[9].can_id = SR439_OBJ_QUALITY_INFO_FL_FRAME_ID;
    // rfilter[9].can_mask = CAN_EFF_MASK; // 扩展帧

    // rfilter[10].can_id = SR439_OBJ_QUALITY_INFO_FR_FRAME_ID;
    // rfilter[10].can_mask = CAN_EFF_MASK; // 扩展帧

    // rfilter[2].can_id = Group1_ID3;
    // rfilter[2].can_mask = CAN_SFF_MASK; // 标准帧
    if (setsockopt(chnl_->fd(), SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)) < 0) {
        throw std::runtime_error(sensor::utils::stringFormat("can socket bind error [%d]%s", errno, strerror(errno)));
    }
}

void CanTransreceiver::send(int frameid, muduo::net::Buffer& buf)
{
    struct canfd_frame frame;
    bzero(&frame, sizeof(frame));

    frame.can_id = frameid;

    frame.len = buf.readableBytes();
    const char* data = buf.peek();
    size_t len = buf.readableBytes();
    for (size_t i = 0; i < len; ++i) {
        frame.data[i] = static_cast<unsigned char>(data[i]);
    }

    if (sizeof(frame) == write(chnl_->fd(), &frame, sizeof(frame))) { // 如果 ret 不等于帧长度，就说明发送失败
        RCLCPP_DEBUG(logger_, "can socket %s send fid: %04x, fsz:%d done!", can_dev_.c_str(), frame.can_id, frame.len);
    } else {
        RCLCPP_WARN(logger_, "can socket %s write error [%d]%s", can_dev_.c_str(), errno, strerror(errno));
    }
}

void CanTransreceiver::setReceiveCallback(CanReceiveCallback cb)
{
    slots_.insert(signal_.connect(std::move(cb)));
}

void CanTransreceiver::onDataReceived()
{
    struct canfd_frame frame;

    int ret = read(chnl_->fd(), &frame, sizeof(struct canfd_frame));
    if (ret < 0) {
        RCLCPP_WARN(logger_, "can socket %s reade error [%d]%s", can_dev_.c_str(), errno, strerror(errno));
        return;
    }

    muduo::net::Buffer buffer;
    for (size_t i = 0; i < frame.len; i++) {
        buffer.appendInt8(frame.data[i]);
    }
    signal_.call(frame.can_id, buffer);
    RCLCPP_INFO(logger_, "can socket %s recv fid: %04x, fsz:%d  done!", can_dev_.c_str(), frame.can_id, frame.len);
}
